#include "broker.h"

#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

struct bcshm_t shm;
struct bcshmseg_t shmseg;

void writer(){
  //while(1){
  for(int i = 0; i < 100; ++i){

    shmseg.ftc_tlm.state_flg = rand() % 100; 
    shmseg.ftc_tlm.timer_1 = rand() % 100000; 
    shmseg.ftc_tlm.timer_2 = rand() % 100000; 
    bcshm_write(SHM_FTC_TLM, &shm, &shmseg.ftc_tlm);

    shmseg.fc_tlm.flight_mode = rand() % 100;
    shmseg.fc_tlm.gps_fix = rand() % 100;
    shmseg.fc_tlm.actuators_count = 6; 
    for(int i = 0; i < 6; ++i)
      shmseg.fc_tlm.actuators[i] = rand() % 2000; 
    shmseg.fc_tlm.hight = rand() % 200;
    shmseg.fc_tlm.speed = rand() % 100;
    bcshm_write(SHM_FC_TLM, &shm, &shmseg.fc_tlm);

    shmseg.bc_tlm.temperature = rand() % 200 - 100;
    bcshm_write(SHM_BC_TLM, &shm, &shmseg.bc_tlm);

    if(i < 99){
      printf("Telemetry:\n");
     
      bcshm_read(SHM_FTC_TLM, &shm, &shmseg.ftc_tlm);
      bcshm_read(SHM_FC_TLM, &shm, &shmseg.fc_tlm);
      bcshm_read(SHM_BC_TLM, &shm, &shmseg.bc_tlm);
     
      printf("\tFTC:\n");
      printf("\t\tState:\t%d\n", (int)shmseg.ftc_tlm.state_flg);
      printf("\t\tTimer1:\t%d\n", (int)shmseg.ftc_tlm.timer_1);
      printf("\t\tTimer2:\t%d\n", (int)shmseg.ftc_tlm.timer_2);
     
      printf("\tFC:\n");
      printf("\t\tFlight mode:\t%d\n", (int)shmseg.fc_tlm.flight_mode);
      printf("\t\tGps fix:\t%d\n", (int)shmseg.fc_tlm.gps_fix);
      printf("\t\tActs count:\t%d\n", (int)shmseg.fc_tlm.actuators_count);
      for(int i = 0; i < 6; ++i)
        printf("\t\t\tmotor %d:\t%d\n", i, (int)shmseg.fc_tlm.actuators[i]);
      printf("\t\tHight:\t%d\n", (int)shmseg.fc_tlm.hight);
      printf("\t\tVelocity:\t%d\n", (int)shmseg.fc_tlm.speed);
     
      printf("\tBC:\n");
      printf("\t\tTemperature:\t%d\n", (int)shmseg.bc_tlm.temperature);
    }
    
    usleep(1e6);
  }
}

int main(int argc, char* argv[]) {
  srand(time(NULL));

  //bcshm_delete();
  bcshm_init();
  bcshm_open(&shm);

  writer();
  
  bcshm_close(&shm);
  return 0;
}
